#include <iostream>
#include <include.hpp>
#include <opencv2/opencv.hpp>
#include <cmath>
#include <fstream>
#include <filesystem>

double b = (0.55/2/M_PI);
double k_0 = (3.41-2*0.275);
double k_1 = (2.2-2*0.275);
double h_rk4 = 0.01;//rk的步长
double h_direct_traversal = 0.0001;//二分法求零点的步长

double theta[224] = {0};
double omega[224] = {0};
cv::RotatedRect rect[223];

// cv::Mat img = cv::Mat::zeros(1000, 2000, CV_8UC3);
cv::Mat img = cv::Mat::zeros(1000, 1000, CV_8UC3);
float k_offset = 40;
// cv::Point2f b_offset = {800, 500};
cv::Point2f b_offset = {500, 500};



// RK4方法，y0为初始条件，t0为初始时间，h为步长，steps为步数
double rk4(double (*f)(const double&, const double&), const double& y0, const double& t0, const double& h, const int& steps)
{
    double t = t0, y = y0;
    for (int i = 0; i < steps; ++i) {
        double k1 = h * f(y, t);
        double k2 = h * f(y + 0.5 * k1, t + 0.5 * h);
        double k3 = h * f(y + 0.5 * k2, t + 0.5 * h);
        double k4 = h * f(y + k3, t + h);
        y += (k1 + 2 * k2 + 2 * k3 + k4) / 6;
        t += h;
    }
    return y;
}

double f_theta_1(const double& theta_1, const double& theta_0, const double& k)
{
    return theta_0*theta_0 + theta_1*theta_1 - 2*theta_1*theta_0*cos(theta_0-theta_1)-k*k/(b*b);
}

double direct_traversal_(double (*f)(const double&, const double&, const double&), const double& x_0, const double& k)
{
    double h = 0.1;//步长
    double x_l = x_0, x_r = x_l + h;
    while (f(x_l, x_0, k) * f(x_r, x_0, k) > 0)
    {
        x_l = x_r;
        x_r += h;
    }

    double mid = (x_l+x_r)/2;
    // double times = 10;//迭代次数
    double epsilon = h_direct_traversal;//收敛区域，越小越精确
    double f_l, f_r, f_mid;
    while (std::abs(x_l - x_r) > epsilon)
    {
        f_l = f(x_l, x_0, k);
        f_mid = f(mid, x_0, k);
        f_r = f(x_r, x_0, k);
        if (0 == f_mid)
        {
            break;
        }
        else if (f_l*f_mid < 0)
        {
            x_r = mid;
        }
        else
        {
            x_l = mid;
        }
        mid = (x_l+x_r)/2;
    }
    return mid;
}

double dtheta_divided_by_dt(const double& theta, const double& t)
{
    //返回dtheta/dt
    return -1/(b*sqrt(1+theta*theta));
}

//根据theta_0求theta_1
double theta_1_theta_0(const double& theta_0, const double& k)
{
    return direct_traversal_(f_theta_1, theta_0, k);
}

//根据theta_0、theta_1、omega_0求omega_1
double omega_1(const double& theta_0, const double& theta_1, const double& omega_0)
{
    double a = theta_1 - theta_0*cos(theta_0-theta_1) - theta_0*theta_1*sin(theta_0-theta_1);
    double b = omega_0*theta_1*(cos(theta_0-theta_1)-theta_0*sin(theta_0-theta_1))-omega_0*theta_0;
    return b/a;
}

//计算theta_0与t的函数、其它函数
void calculate(const int& start_time, const int& end_time)
{
    double t_0 = start_time, t_end = end_time, h = h_rk4;//步长
    int steps = static_cast<int>((t_end - t_0) / h); // 计算步数

    // std::cout << "94" << std::endl;
    // arr_theta_0_t[0] = theta[0];
    double t = 0;
    bool flg_to_end = false;
    for (int i = 0; i < steps; ++i)//求得(i+1)*h时刻第零个点的角度
    // for (int i = 0; i < 1; ++i)//求得(i+1)*h时刻第零个点的角度
    {
        t = (i+1)*h;
        img = cv::Mat::zeros(1000, 1000, CV_8UC3);

        if (0 == i)
        {
            // std::cout << "103" << std::endl;
            theta[0] = 32 * M_PI;//第0个点的角度
            // std::cout << "105" << std::endl;
        }
        else
        {
            theta[0] = rk4(dtheta_divided_by_dt, theta[0], t_0, h, 1);//求得(i+1)*h时刻，第0个点的角度
            // arr_theta_0_t[(int)(i*h)+1] = theta[0];//求得整数时刻第0个点的角度
        }
        omega[0] = -1/b/sqrt(1+theta[0]*theta[0]);//求得(i+1)*h时刻，第0个点的角速度


        // std::cout << "112" << std::endl;
        theta[1] = theta_1_theta_0(theta[0], k_0);//第1个点的角度
        omega[1] = omega_1(theta[0], theta[1], omega[0]);//第1个点的角速度
        // std::cout << "114" << std::endl;
        cv::Point2f X_0(b*theta[0]*cos(theta[0]), b*theta[0]*sin(theta[0]));
        cv::Point2f X_1(b*theta[1]*cos(theta[1]), b*theta[1]*sin(theta[1]));
        cv::Point2f vec_X = X_1 - X_0;
        cv::Point2f X_mid = (X_0 + X_1)/2;
        // std::cout << "X_0: " << X_0 << std::endl;
        // std::cout << "X_1: " << X_1 << std::endl;
        // std::cout << "X_mid: " << X_mid << std::endl;
        // std::cout << "vec_X: " << vec_X << std::endl;
        double angle_of_rect = (atan2(vec_X.y, vec_X.x)) * 180 / M_PI;//求得矩形的旋转角度（顺时针为正）
        // double angle_of_rect = 0;
        // std::cout << "angle_of_rect: " << angle_of_rect << std::endl;
        // rect[0] = cv::RotatedRect(cv::Point2f(250, 250), cv::Size2f(300, 100), 0);//求得的矩形即为龙头
        rect[0] = cv::RotatedRect(X_mid, cv::Size2f(3.41, 0.3), angle_of_rect);//求得的矩形即为龙头

        cv::Point2f head_vertices[4];
        rect[0].points(head_vertices);
        // std::cout << head_vertices[0] << ", " << head_vertices[1] << ", " << head_vertices[2] << ", " << head_vertices[3] << std::endl;
        std::array<cv::Point2f, 4> vec_head_vertices = {head_vertices[0], head_vertices[1], head_vertices[2], head_vertices[3]};
        // std::cout << "vec_head_vertices: " << std::endl;
        // std::cout << vec_head_vertices[0] << ", " << vec_head_vertices[1] << ", " << vec_head_vertices[2] << ", " << vec_head_vertices[3] << std::endl;

        for (int n = 0; n < 4; ++n)
        {
            // std::cout << "before: " << head_vertices[n];
            head_vertices[n] = k_offset * head_vertices[n];
            head_vertices[n].y *= -1;
            head_vertices[n] = head_vertices[n] + b_offset;
            // std::cout << ", after: " << head_vertices[n] << std::endl;
        }
        for (int n = 0; n < 4; ++n)
        {
            cv::line(img, head_vertices[n], head_vertices[(n + 1) % 4], cv::Scalar(0, 255, 0), 1);
        }


        for (int j = 2; j < 224; ++j)//求得(i+1)*h时刻后面所有点的角度
        {
            // std::cout << "128" << std::endl;
            theta[j] = theta_1_theta_0(theta[j-1], k_1);
            omega[j] = omega_1(theta[j-1], theta[j], omega[j-1]);
            // std::cout << "theta[" << j << "]: " << theta[j] << std::endl;
            // std::cout << b*theta[j]*cos(theta[j]) << ", " << b*theta[j]*sin(theta[j]) << std::endl;
            cv::Point2f X_0(b*theta[j-1]*cos(theta[j-1]), b*theta[j-1]*sin(theta[j-1]));
            cv::Point2f X_1(b*theta[j]*cos(theta[j]), b*theta[j]*sin(theta[j]));
            cv::Point2f vec_X = X_1 - X_0;
            cv::Point2f X_mid = (X_0 + X_1)/2;
            double angle_of_rect = (atan2(vec_X.y, vec_X.x)) * 180 / M_PI;//求得矩形的旋转角度（顺时针为正）
            rect[j-1] = cv::RotatedRect(X_mid, cv::Size2f(2.2, 0.3), angle_of_rect);//求得第j-1个矩形
            // std::cout << "longtou: " << rect[0].center << std::endl;
            // std::cout << rect[j-1].center << std::endl;
            // std::cout << "135" << std::endl;
            //接下来判断龙头的四个点是否在后面若干个矩形的内部
            cv::Point2f vertices[4];
            rect[j-1].points(vertices);
            std::array<cv::Point2f, 4> vec_vertices = {vertices[0], vertices[1], vertices[2], vertices[3]};
            for (int n = 0; n < 4; ++n)
            {
                vertices[n] = k_offset * vertices[n];
                vertices[n].y *= -1;
                vertices[n] = vertices[n] + b_offset;
            }
            for (int n = 0; n < 4; ++n)
            {
                cv::line(img, vertices[n], vertices[(n + 1) % 4], cv::Scalar(255, 0, 0), 1);
            }

            // std::cout << j-1 << ": vec_vertices: ";
            for (int n = 0; n < 4; ++n)
            {
                double dis = cv::pointPolygonTest(vec_vertices, vec_head_vertices[n], true);
                
                // std::cout << vec_vertices[n] << ", ";
                // std::cout << "flg: " << flg << "; ";
                if (j != 2 && dis >= 0)
                {
                    std::cout << "在第" << t << "s时碰撞！" << std::endl;
                    flg_to_end = true;
                }
            }
            // std::cout << std::endl;

        }

        if (true == flg_to_end)
        {
            std::ofstream result2("/home/zwh/myStudies/C++/model_2/result2.csv");
            for (int n = 0; n < 224; ++n)
            {
                result2 << b*theta[n]*cos(theta[n]) << "," << b*theta[n]*sin(theta[n]) << ',' << std::abs(b*sqrt(1+theta[n]*theta[n])*omega[n]) << '\n';
            }
            result2.close();
            return;
        }
        cv::imshow("img", img);
        cv::waitKey(1);



        // for (int i = 0; i < 224; ++i)
        // {
            // std::cout << "x: " << b*theta[i]*cos(theta[i]) << ", y: " << b*theta[i]*sin(theta[i]) << std::endl;
            // std::cout << "dis: " << b*sqrt(pow(theta[i+1]*cos(theta[i+1])-theta[i]*cos(theta[i]), 2) + pow(theta[i+1]*sin(theta[i+1])-theta[i]*sin(theta[i]), 2)) << std::endl;
        //     cv::Point2f tmp;
        //     tmp.x = b*theta[i]
        //     std::cout << "theta[" << i << "]: " << theta[i] << std::endl;
        // }
        
        std::cout << "现在的时间是" << t << std::endl;

    }

}


int main(void)
{
    calculate(0, 500);
    return 0;
}